/******************************************************************************
 * Copyright 2022 The AIR Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "v2x-asn-msgs-internal.h"

namespace {
const Position3D_t _s_position_zero{};
}

bool position_pb_llh2xyz(                   //
    const ::v2xpb::asn::Position_LLH &llh,  //
    ::v2xpb::asn::Position_XYZ *xyz         //
) {
  if (!xyz) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (!llh.has_latitude() || !llh.has_longitude() ||  //
      std::isnan(llh.latitude()) || std::isnan(llh.longitude())) {
    LOG(ERROR) << "Invalid LLH";
    return false;
  }
  const double lat = llh.latitude();
  const double lon = llh.longitude();
  int zone = 0;
  double x = 0, y = 0;
  try {
    bool dummy = false;
    GeographicLib::UTMUPS::Forward(lat, lon, zone, dummy, x, y);
  } catch (const std::exception &e) {
    LOG(ERROR) << "Failed to convert position: " << e.what();
    return false;
  }
  xyz->set_zone(zone);
  xyz->set_x(x);
  xyz->set_y(y);
  if (llh.has_elevation() && llh.elevation() != NAN) {
    xyz->set_z(llh.elevation());
  }
  return true;
}

bool position_pb_xyz2llh(                   //
    const ::v2xpb::asn::Position_XYZ &xyz,  //
    ::v2xpb::asn::Position_LLH *llh         //
) {
  if (!llh) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (!xyz.has_zone() || !xyz.has_x() || !xyz.has_y()) {
    LOG(ERROR) << "Invalid XYZ";
    return false;
  }
  double lat = NAN;
  double lon = NAN;
  try {
    GeographicLib::UTMUPS::Reverse(xyz.zone(), true, xyz.x(), xyz.y(), lat,
                                   lon);
  } catch (const std::exception &e) {
    LOG(ERROR) << "Failed to convert position: " << e.what();
    return false;
  }
  llh->set_latitude(lat);
  llh->set_longitude(lon);
  if (xyz.has_z()) {
    llh->set_elevation(xyz.z());
  }
  return true;
}

bool position_asn2pb_llh(                     //
    const Position3D_t *pos_asn,              //
    ::v2xpb::asn::Position_LLH *position_llh  //
) {
  if (!pos_asn || !position_llh) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  position_llh->set_latitude(static_cast<double>(pos_asn->lat) / 1e7);
  position_llh->set_longitude(static_cast<double>(pos_asn->Long) / 1e7);
  if (nullptr != pos_asn->elevation &&
      *pos_asn->elevation != V2X_ASN_ELEVATION_MIN) {
    // V2X_ASN_ELEVATION_MIN means UNAVAILABLE
    position_llh->set_elevation(static_cast<double>(*pos_asn->elevation) / 10);
  }
  return true;
}

bool position_pb2asn_llh(                            //
    const ::v2xpb::asn::Position_LLH &position_llh,  //
    Position3D_t *pos_asn                            //
) {
  if (!pos_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (!position_llh.has_latitude() || !position_llh.has_longitude() ||  //
      std::isnan(position_llh.latitude()) ||
      std::isnan(position_llh.longitude())) {
    LOG(ERROR) << "Invalid LLH";
    return false;
  }
  pos_asn->lat = static_cast<double>(
      round(position_llh.latitude() * 1e7));  // unit: 1E-7 degree
  pos_asn->Long = static_cast<double>(
      round(position_llh.longitude() * 1e7));  // unit: 1E-7 degree
  pos_asn->elevation = ASN1(Elevation)::create_tp();
  *pos_asn->elevation = V2X_ASN_ELEVATION_MIN;
  if (!position_llh.has_elevation() || std::isnan(position_llh.elevation())) {
    return true;
  }
  auto elev =
      static_cast<double>(round(position_llh.elevation() * 10));  // unit: 0.1m
  if (elev <= V2X_ASN_ELEVATION_MAX && elev >= V2X_ASN_ELEVATION_MIN) {
    *pos_asn->elevation = elev;
  }
  return true;
}

bool position_asn2pb_llh(                     //
    const Position3D_t *pos_center,           //
    const PositionOffsetLLV_t *pos_asn,       //
    ::v2xpb::asn::Position_LLH *position_llh  //
) {
  if (!pos_center || !pos_asn || !position_llh) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  int64_t lat_e7 = pos_center->lat;
  int64_t lon_e7 = pos_center->Long;
  switch (pos_asn->offsetLL.present) {
    case PositionOffsetLL_PR_position_LL1:
      lat_e7 += pos_asn->offsetLL.choice.position_LL1.lat;
      lon_e7 += pos_asn->offsetLL.choice.position_LL1.lon;
      break;
    case PositionOffsetLL_PR_position_LL2:
      lat_e7 += pos_asn->offsetLL.choice.position_LL2.lat;
      lon_e7 += pos_asn->offsetLL.choice.position_LL2.lon;
      break;
    case PositionOffsetLL_PR_position_LL3:
      lat_e7 += pos_asn->offsetLL.choice.position_LL3.lat;
      lon_e7 += pos_asn->offsetLL.choice.position_LL3.lon;
      break;
    case PositionOffsetLL_PR_position_LL4:
      lat_e7 += pos_asn->offsetLL.choice.position_LL4.lat;
      lon_e7 += pos_asn->offsetLL.choice.position_LL4.lon;
      break;
    case PositionOffsetLL_PR_position_LL5:
      lat_e7 += pos_asn->offsetLL.choice.position_LL5.lat;
      lon_e7 += pos_asn->offsetLL.choice.position_LL5.lon;
      break;
    case PositionOffsetLL_PR_position_LL6:
      lat_e7 += pos_asn->offsetLL.choice.position_LL6.lat;
      lon_e7 += pos_asn->offsetLL.choice.position_LL6.lon;
      break;
    case PositionOffsetLL_PR_position_LatLon:
      lat_e7 = pos_asn->offsetLL.choice.position_LatLon.lat;
      lon_e7 = pos_asn->offsetLL.choice.position_LatLon.lon;
      break;
    default:
      LOG(ERROR) << "Invalid position ll_offset present: "
                 << pos_asn->offsetLL.present;
      return false;
  }
  position_llh->set_latitude(static_cast<double>(lat_e7) / 1e7);
  position_llh->set_longitude(static_cast<double>(lon_e7) / 1e7);

  if (!pos_asn->offsetV) {
    return true;
  }
  if (!pos_center->elevation ||
      *pos_center->elevation == V2X_ASN_ELEVATION_MIN) {
    if (pos_asn->offsetV->present != VerticalOffset_PR_elevation ||
        pos_asn->offsetV->choice.elevation == V2X_ASN_ELEVATION_MIN) {
      return true;
    }
    double elevation =
        static_cast<double>(pos_asn->offsetV->choice.elevation) / 10;
    position_llh->set_elevation(elevation);
    return true;
  }
  int64_t z_e1 = *pos_center->elevation;
  switch (pos_asn->offsetV->present) {
    case VerticalOffset_PR_offset1:
      z_e1 += pos_asn->offsetV->choice.offset1;
      break;
    case VerticalOffset_PR_offset2:
      z_e1 += pos_asn->offsetV->choice.offset2;
      break;
    case VerticalOffset_PR_offset3:
      z_e1 += pos_asn->offsetV->choice.offset3;
      break;
    case VerticalOffset_PR_offset4:
      z_e1 += pos_asn->offsetV->choice.offset4;
      break;
    case VerticalOffset_PR_offset5:
      z_e1 += pos_asn->offsetV->choice.offset5;
      break;
    case VerticalOffset_PR_offset6:
      z_e1 += pos_asn->offsetV->choice.offset6;
      break;
    case VerticalOffset_PR_elevation:
      if (V2X_ASN_ELEVATION_MIN == pos_asn->offsetV->choice.elevation) {
        return true;
      }
      z_e1 = pos_asn->offsetV->choice.elevation;
      break;
    default:
      LOG(WARNING) << "Invalid position v_offset present: "
                   << pos_asn->offsetLL.present << " Ignored.";
      return true;
  }
  double elevation = static_cast<double>(z_e1) / 10;
  position_llh->set_elevation(elevation);
  return true;
}

bool position_pb2asn_llh(const Position3D_t *pos_center,                  //
                         const ::v2xpb::asn::Position_LLH &position_llh,  //
                         PositionOffsetLLV_t *pos_asn                     //
) {
  if (!pos_center || !pos_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (!position_llh.has_latitude() || !position_llh.has_longitude() ||  //
      std::isnan(position_llh.latitude()) ||
      std::isnan(position_llh.longitude())) {
    LOG(ERROR) << "Invalid LLH";
    return false;
  }
  int64_t lat_orig = static_cast<double>(
      round(position_llh.latitude() * 1e7));  // unit: 1E-7 degree
  int64_t lon_orig = static_cast<double>(
      round(position_llh.longitude() * 1e7));  // unit: 1E-7 degree

  int64_t lat_diff = lat_orig - pos_center->lat;
  int64_t lon_diff = lon_orig - pos_center->Long;

  if (abs(lat_diff) < (1 << 11) && abs(lon_diff) < (1 << 11)) {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LL1;
    pos_asn->offsetLL.choice.position_LL1.lat = lat_diff;
    pos_asn->offsetLL.choice.position_LL1.lon = lon_diff;
  } else if (abs(lat_diff) < (1 << 13) && abs(lon_diff) < (1 << 13)) {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LL2;
    pos_asn->offsetLL.choice.position_LL2.lat = lat_diff;
    pos_asn->offsetLL.choice.position_LL2.lon = lon_diff;
  } else if (abs(lat_diff) < (1 << 15) && abs(lon_diff) < (1 << 15)) {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LL3;
    pos_asn->offsetLL.choice.position_LL3.lat = lat_diff;
    pos_asn->offsetLL.choice.position_LL3.lon = lon_diff;
  } else if (abs(lat_diff) < (1 << 17) && abs(lon_diff) < (1 << 17)) {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LL4;
    pos_asn->offsetLL.choice.position_LL4.lat = lat_diff;
    pos_asn->offsetLL.choice.position_LL4.lon = lon_diff;
  } else if (abs(lat_diff) < (1 << 21) && abs(lon_diff) < (1 << 21)) {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LL5;
    pos_asn->offsetLL.choice.position_LL5.lat = lat_diff;
    pos_asn->offsetLL.choice.position_LL5.lon = lon_diff;
  } else if (abs(lat_diff) < (1 << 23) && abs(lon_diff) < (1 << 23)) {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LL6;
    pos_asn->offsetLL.choice.position_LL6.lat = lat_diff;
    pos_asn->offsetLL.choice.position_LL6.lon = lon_diff;
  } else {
    pos_asn->offsetLL.present = PositionOffsetLL_PR_position_LatLon;
    pos_asn->offsetLL.choice.position_LatLon.lat = lat_orig;
    pos_asn->offsetLL.choice.position_LatLon.lon = lon_orig;
  }
  if (!position_llh.has_elevation() || std::isnan(position_llh.elevation())) {
    return true;
  }
  int64_t z_center = INT32_MIN;
  int64_t z_orig =
      static_cast<double>(round(position_llh.elevation() * 10));  // unit: 0.1m
  if (pos_center->elevation != nullptr &&
      *pos_center->elevation !=
          V2X_ASN_ELEVATION_MIN) {  // ELEVATION_MIN means UNAVAILABLE
    z_center = *pos_center->elevation;
  }
  int64_t z_diff = z_orig - z_center;
  pos_asn->offsetV = ASN1(VerticalOffset)::create_tp();
  if (abs(z_diff) < (1 << 6)) {
    pos_asn->offsetV->present = VerticalOffset_PR_offset1;
    pos_asn->offsetV->choice.offset1 = z_diff;
  } else if (abs(z_diff) < (1 << 7)) {
    pos_asn->offsetV->present = VerticalOffset_PR_offset2;
    pos_asn->offsetV->choice.offset2 = z_diff;
  } else if (abs(z_diff) < (1 << 8)) {
    pos_asn->offsetV->present = VerticalOffset_PR_offset3;
    pos_asn->offsetV->choice.offset3 = z_diff;
  } else if (abs(z_diff) < (1 << 9)) {
    pos_asn->offsetV->present = VerticalOffset_PR_offset4;
    pos_asn->offsetV->choice.offset4 = z_diff;
  } else if (abs(z_diff) < (1 << 10)) {
    pos_asn->offsetV->present = VerticalOffset_PR_offset5;
    pos_asn->offsetV->choice.offset5 = z_diff;
  } else if (abs(z_diff) < (1 << 11)) {
    pos_asn->offsetV->present = VerticalOffset_PR_offset6;
    pos_asn->offsetV->choice.offset6 = z_diff;
  } else {
    pos_asn->offsetV->present = VerticalOffset_PR_elevation;
    pos_asn->offsetV->choice.elevation = z_orig;
    if (pos_asn->offsetV->choice.elevation < V2X_ASN_ELEVATION_MIN ||
        pos_asn->offsetV->choice.elevation > V2X_ASN_ELEVATION_MAX) {
      pos_asn->offsetV->choice.elevation = V2X_ASN_ELEVATION_MIN;
    }
  }
  return true;
}

bool position_asn2pb(                 //
    const Position3D_t *pos_asn,      //
    ::v2xpb::asn::Position *position  //
) {
  if (!pos_asn || !position) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  return position_asn2pb_llh(pos_asn, position->mutable_llh()) &&
         position_pb_llh2xyz(position->llh(), position->mutable_xyz());
}

bool position_asn2pb(                    //
    const Position3D_t *pos_center,      //
    const PositionOffsetLLV_t *pos_asn,  //
    ::v2xpb::asn::Position *position     //
) {
  if (!pos_center || !pos_asn || !position) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  return position_asn2pb_llh(pos_center, pos_asn, position->mutable_llh()) &&
         position_pb_llh2xyz(position->llh(), position->mutable_xyz());
}

bool position_pb2asn(                        //
    const ::v2xpb::asn::Position &position,  //
    Position3D_t *pos_asn                    //
) {
  if (!pos_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (position.has_llh()) {
    if (position_pb2asn_llh(position.llh(), pos_asn)) {
      return true;
    }
  }
  ::v2xpb::asn::Position_LLH llh;
  if (position.has_xyz()) {
    if (!position_pb_xyz2llh(position.xyz(), &llh)) {
      return false;
    }
    return position_pb2asn_llh(llh, pos_asn);
  }
  return false;
}

bool position_pb2asn(                        //
    const Position3D_t *pos_center,          //
    const ::v2xpb::asn::Position &position,  //
    PositionOffsetLLV_t *pos_asn             //
) {
  if (!pos_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (!pos_center) {
    pos_center = &_s_position_zero;
  }
  if (position.has_llh()) {
    if (position_pb2asn_llh(pos_center, position.llh(), pos_asn)) {
      return true;
    }
  }
  ::v2xpb::asn::Position_LLH llh;
  if (position.has_xyz()) {
    if (!position_pb_xyz2llh(position.xyz(), &llh)) {
      return false;
    }
    return position_pb2asn_llh(pos_center, llh, pos_asn);
  }
  return false;
}
